#include "dragTrick.h"


//--------------------------------------------------------------
void dragTrick::setup(){

	bTakeSnapshot 					= false;
	bHaveBlobs 						= false;
	bTakeTopLeftmostBlob 			= true;
	
	offset.x 						= 0;
	offset.y 						= 0;
	whoGotMe 						= -1;
	
	
	bPlaying						= false;
	playbackCounter					= 0;
	bShow 							= false;
	
	
	meAsGray.allocate(320,240);
	meAndedWithHand.allocate(320,240);
	
	
	
}

void dragTrick::reset(){
	points.clear();
	bTakeSnapshot 					= false;
	bHaveBlobs 						= false;
	bTakeTopLeftmostBlob 			= true;
	
	offset.x 						= 0;
	offset.y 						= 0;
	whoGotMe 						= -1;
	
	
	bPlaying						= false;
	playbackCounter					= 0;
	bShow							= true;
	
}

//--------------------------------------------------------------
void dragTrick::update(){
	
	sprintf(name, "drag");
	
	if (bTakeSnapshot == true){
		
		
		vbd->takeSnapshot();
		
		if (bTakeTopLeftmostBlob == true){
			float smallestDistance 			= 1000000;
			int indexOfTopLeftmostBlob 		= -1;
			for (int i = 0; i < vbd->nBlobs; i++){
				float dist = sqrt(  vbd->vidBlob[i].pos.x * vbd->vidBlob[i].pos.x + 
									vbd->vidBlob[i].pos.y * vbd->vidBlob[i].pos.y);
				if (dist < smallestDistance){ 
					smallestDistance = dist;
					indexOfTopLeftmostBlob = i;
				}
			}
			
			if (indexOfTopLeftmostBlob != -1){
				if (vbd->nBlobs > 0){
					memcpy(&blob,&vbd->vidBlob[indexOfTopLeftmostBlob], sizeof(videoBlob));
					bHaveBlobs = true;
				}
				bHaveBlobs = true;
			}
			 		
		} else {
			if (vbd->nBlobs > 0){
				memcpy(&blob,&vbd->vidBlob, sizeof(videoBlob));
				bHaveBlobs = true;
			}
		}
		bTakeSnapshot = false;
		bShow = true;
	}
	
	
	
	
	
	
	
	
	//----------------------
	if (bHaveBlobs == true){
	
		for (int i = 0; i < MAX_N_TRACKED_FINGERS; i++){
			fingerInMeEnergy[i] *= 0.97f;
		}		
	
		blob.update();
		//------------------ ok now let's check fingers in the blob:
		for (int i = 0; i < MAX_N_TRACKED_FINGERS; i++){
			if (vhd->fingers[i].energy < 0.05f){
				fingerInMeEnergy[i] = 0;
			} else {
				float x = vhd->fingers[i].pos.x;
				float y = vhd->fingers[i].pos.y;
				if (blob.pointInsideMe(x, y)){
					fingerInMeEnergy[i] += 0.05f;
				}
				
			}
		}
		
		
		
		if (bPlaying == false){
			
			if (whoGotMe == -1){
				for (int i = 0; i < MAX_N_TRACKED_FINGERS; i++){
					if (fingerInMeEnergy[i] > 0.9f){
						whoGotMe = i;
						offset.x = vhd->fingers[i].pos.x - blob.pos.x;
						offset.y = vhd->fingers[i].pos.y - blob.pos.y;
						break;
						
						prevPoint.x = blob.pos.x;
						prevPoint.y = blob.pos.y;
						vel = 0;
	
					}
				}
			} else {
				if (fingerInMeEnergy[whoGotMe] < 0.01){
					whoGotMe = -1;
					bPlaying = true;
					playbackCounter = points.size() - 1;
				} else {
				
					float newx = 0.5f * blob.pos.x + 0.5f * (vhd->fingers[whoGotMe].pos.x - offset.x);
					float newy = 0.5f * blob.pos.y + 0.5f * (vhd->fingers[whoGotMe].pos.y - offset.y);

					float dx 		= newx - prevPoint.x;
					float dy 		= newy - prevPoint.y;
					float myVel 	= sqrt(dx*dx + dy*dy); 
					vel 			= 0.9f * vel + 0.1f * myVel;
					
					bool bVelOk = true;
					if (vel > 10 ||  ((points.size() > 30) && (vel < 0.25f)) ){
						bVelOk = false;
					}
					
					if (bVelOk){
						blob.pos.x = newx; //0.5f * blob.pos.x + 0.5f * (vhd->fingers[whoGotMe].pos.x - offset.x);
						blob.pos.y = newy; //0.5f * blob.pos.y + 0.5f * (vhd->fingers[whoGotMe].pos.y - offset.y);
						points.push_back(blob.pos);
					} else {
						whoGotMe = -1;
						bPlaying = true;
						playbackCounter = points.size() - 1;
					}
					prevPoint.x = blob.pos.x;
					prevPoint.y = blob.pos.y;
					
					
				}
			}
			
		} else {
		
			playbackCounter--;
			if (playbackCounter < 0){
				bPlaying = false;
				points.clear();
			} else {
				blob.pos.x = points[playbackCounter].x;
				blob.pos.y = points[playbackCounter].y;
			}
		
		
		}
		
	} // have blob
	
	
	meAsGray.set(0);
	meAndedWithHand.set(0);
	
	
	if (bHaveBlobs == true){
		//blob.angle = ofRandom(0,TWO_PI);
		blob.draw(meAsGray, 255);
		meAndedWithHand = meAsGray;
		meAndedWithHand &= vhd->handDetectionImage;
		
		
		int meCount = meAsGray.countNonZeroInRegion(0,0,width, height);
		int andCount = meAndedWithHand.countNonZeroInRegion(0,0,width, height);
		
		if (meCount == andCount){
			nFramesCovered++;
			//bUncovered = false;
			if (bUncovered == true && nFramesCovered > 5){
				bShow = !bShow;
				bUncovered = false;
			}
		} else {
			nFramesCovered = 0;
		}
		
		if (andCount == 0){
			bUncovered = true;
		}
	}
	
}

//--------------------------------------------------------------
void dragTrick::draw(){
	if (bShow == true){
		if (bHaveBlobs == true){
			blob.draw();
		}
	} else {
		if (bHaveBlobs == true){
			ofEnableAlphaBlending();
			ofSetColor(255,0,0,155);
			if (bDrawDiagnostically) blob.drawOutline();
			ofDisableAlphaBlending();
			ofSetColor(0xffffff);
		}
	}
}


//--------------------------------------------------------------
void dragTrick::keyPressed(int key){
	if (key == 's'){
		bTakeSnapshot = true;
	} else if (key == '0'){
		bShow = !bShow;
	} else if (key == ' '){
		reset();
	}
}